clear all
close all
qbit=1;showfig = 0;

Nsub=256;   % no. of subcarriers for rate computation
Ngrid = 1000;
L = 2;

factor = 2;
x1 = 40e-1*factor;
y1 = 50e-1*ones(size(factor));
z1 = 90e-1*factor;
x2 = -30e-1*factor;
y2 = 50e-1*ones(size(factor));
z2 = 40e-1*factor;
TRX = zeros(6,length(factor));
TRX(1,:) = x1;
TRX(2,:) = y1;
TRX(3,:) = z1;
TRX(4,:) = x2;
TRX(5,:) = y2;
TRX(6,:) = z2;
dist = rms(TRX(1:3,:))*sqrt(3)+rms(TRX(4:6,:))*sqrt(3);

fc = 30e9;
c = 3e8;
BW_des = 1.5e9;
Ptx = 10;%dBm
Rates = zeros(12,length(Ptx));
Ntest = 100;
% delta_r = 0:0.1:1;
delta_r = [];
delta_theta = 0:0.001:0.005;
% delta = 0.5;
for n = 1:length(delta_r)
    d.r = delta_r(n);
    d.theta = 0;
    d.phi = 0;
   Rates(1:3,n) = my_compute_rate_err(10,Nsub,L,TRX,fc,BW_des,Ptx,d,Ntest); 
end
for n  =1:length(delta_theta)
   d.r = 0;
    d.theta = delta_theta(n)/sqrt(2);
    d.phi = delta_theta(n)/sqrt(2);
   Rates(4:6,n) = my_compute_rate_err(10,Nsub,L,TRX,fc,BW_des,Ptx,d,Ntest); 
end

subplot(2,1,1)
plot(delta_r,Rates(3,1:length(delta_r)),'--','LineWidth',1.5) % optimal
hold on
plot(delta_r,Rates(2,1:length(delta_r)),'d-','LineWidth',1.5) % proposed
plot(delta_r,Rates(1,1:length(delta_r)),'k^--','LineWidth',1.5) % classial 
xlabel('Standard deviation of error in estimated distance (m)')
ylabel('Average rate per subcarrier (bit/s/Hz)')
legend('Optimal beamforming','Proposed method','Classical beamforming',...
    'Location','southwest')
hold off

subplot(2,1,2)
plot(delta_theta,Rates(6,1:length(delta_theta)),'--','LineWidth',1.5) % optimal
hold on
plot(delta_theta,Rates(5,1:length(delta_theta)),'d-','LineWidth',1.5) % proposed
plot(delta_theta,Rates(4,1:length(delta_theta)),'k^--','LineWidth',1.5) % classial 
xlabel('Standard deviation of error in estimated angle (m)')
ylabel('Average rate per subcarrier (bit/s/Hz)')
legend('Optimal beamforming','Proposed method','Classical beamforming',...
    'Location','southwest')
hold off

